{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "from local_plot import *\n",
    "from utils import *\n",
    "from trajectory import *\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def avg_distance(paths, nodes):\n",
    "    total = []\n",
    "    for i in nodes:\n",
    "        for j in nodes:\n",
    "            if i != j:\n",
    "                t = find_common_times(paths[i].t, paths[j].t)\n",
    "                total.append(AVG_DIS(paths[i].pos_func(t), paths[j].pos_func(t)))\n",
    "    return np.mean(total)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [3, 4]\n",
    "output_folder = f\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26//outputs/d2vins-2-sync/swarm{nodes[0]}\"\n",
    "data_folder=\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26/\"\n",
    "dte = 140\n",
    "paths_gt, t0 = read_paths(data_folder, nodes, prefix=\"manual_small_\", suffix=\"-groundtruth.txt\")\n",
    "paths, _ = read_paths(output_folder, nodes, t0=t0)\n",
    "# paths2, _ = read_paths(\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26//outputs/d2vins-2/swarm2\", nodes, t0=t0)\n",
    "# paths[2] = paths2[2]\n",
    "print(f\"Avg distance {avg_distance(paths_gt, nodes):.2f}m\")\n",
    "align_paths(paths, paths_gt, True)\n",
    "display(plot_fused_err(nodes, paths, paths_gt, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths, paths_gt, dte=dte, outlier_thres=0.15))\n",
    "# plot_fused(nodes, paths, poses_gt=paths_gt, figsize=(10,10))\n",
    "\n",
    "paths_vo, t0 = read_paths(data_folder, nodes, prefix=\"vins-mono_\", t0=t0)\n",
    "print(\"VINS-Mono\")\n",
    "align_paths(paths_vo, paths_gt)\n",
    "display(plot_fused_err(nodes, paths_vo, paths_gt, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths_vo, paths_gt, dte=dte))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [3, 4]\n",
    "output_folder = f\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26//outputs/d2vins-2-async/swarm{nodes[0]}\"\n",
    "data_folder=\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26/\"\n",
    "dte = 140\n",
    "paths_gt, t0 = read_paths(data_folder, nodes, prefix=\"manual_small_\", suffix=\"-groundtruth.txt\")\n",
    "paths, _ = read_paths(output_folder, nodes, t0=t0)\n",
    "align_paths(paths, paths_gt, True)\n",
    "display(plot_fused_err(nodes, paths, paths_gt, dte=dte, show=False))\n",
    "display(plot_relative_pose_err(nodes[0], nodes[1:], paths, paths_gt, dte=dte, show=False, outlier_thres=0.15))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 3, 4]\n",
    "output_folder = f\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26//outputs/d2vins-3-sync/swarm{nodes[0]}\"\n",
    "data_folder=\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26/\"\n",
    "dte = 140\n",
    "paths_gt, t0 = read_paths(data_folder, nodes, prefix=\"manual_small_\", suffix=\"-groundtruth.txt\")\n",
    "paths, _ = read_paths(output_folder, nodes, t0=t0)\n",
    "print(f\"Avg distance {avg_distance(paths_gt, nodes):.2f}m\")\n",
    "align_paths(paths, paths_gt, align_by_first=True, align_with_minize=False)\n",
    "display(plot_fused_err(nodes, paths, paths_gt, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths, paths_gt, dte=dte, outlier_thres=0.15))\n",
    "# plot_fused(nodes, paths, poses_gt=paths_gt, figsize=(10,10))\n",
    "\n",
    "paths_vo, t0 = read_paths(data_folder, nodes, prefix=\"vins-mono_\", t0=t0)\n",
    "print(\"VINS-Mono\")\n",
    "align_paths(paths_vo, paths_gt)\n",
    "display(plot_fused_err(nodes, paths_vo, paths_gt, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths_vo, paths_gt, dte=dte))\n",
    "# plot_fused(nodes, paths_vo, poses_gt=paths_gt, figsize=(10,10))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 3, 4]\n",
    "output_folder = f\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26//outputs/d2vins-3-async/swarm{nodes[0]}\"\n",
    "data_folder=\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26/\"\n",
    "dte = 140\n",
    "paths_gt, t0 = read_paths(data_folder, nodes, prefix=\"manual_small_\", suffix=\"-groundtruth.txt\")\n",
    "paths, _ = read_paths(output_folder, nodes, t0=t0)\n",
    "align_paths(paths, paths_gt, align_by_first=True, align_with_minize=False)\n",
    "display(plot_fused_err(nodes, paths, paths_gt, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths, paths_gt, dte=dte, outlier_thres=0.15))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2, 3, 4]\n",
    "output_folder = f\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26//outputs/d2vins-4-sync/swarm{nodes[0]}\"\n",
    "data_folder=\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26/\"\n",
    "dte = 140\n",
    "paths_gt, t0 = read_paths(data_folder, nodes, prefix=\"manual_small_\", suffix=\"-groundtruth.txt\")\n",
    "paths, _ = read_paths(output_folder, nodes, t0=t0)\n",
    "# print(f\"Avg distance {avg_distance(paths_gt, nodes):.2f}m\")\n",
    "align_paths(paths, paths_gt, align_by_first=True, align_with_minize=False)\n",
    "display(plot_fused_err(nodes, paths, paths_gt, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths, paths_gt, dte=dte, outlier_thres=0.2, outlier_yaw_thres=0.05))\n",
    "\n",
    "paths_vo, t0 = read_paths(data_folder, nodes, prefix=\"vins-mono_\", t0=t0)\n",
    "print(\"VINS-Mono\")\n",
    "align_paths(paths_vo, paths_gt)\n",
    "display(plot_fused_err(nodes, paths_vo, paths_gt, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths_vo, paths_gt, dte=dte))\n",
    "# plot_fused(nodes, paths_vo, poses_gt=paths_gt, figsize=(10,10))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2, 3, 4]\n",
    "output_folder = f\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26//outputs/d2vins-4-async/swarm{nodes[0]}\"\n",
    "data_folder=\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26/\"\n",
    "dte = 140\n",
    "paths_gt, t0 = read_paths(data_folder, nodes, prefix=\"manual_small_\", suffix=\"-groundtruth.txt\")\n",
    "paths, _ = read_paths(output_folder, nodes, t0=t0)\n",
    "align_paths(paths, paths_gt, align_by_first=True, align_with_minize=False)\n",
    "display(plot_fused_err(nodes, paths, paths_gt, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths, paths_gt, dte=dte, outlier_thres=0.2, outlier_yaw_thres=0.05))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2, 3, 4, 5]\n",
    "output_folder = f\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26//outputs/d2vins-5-sync/swarm{nodes[0]}\"\n",
    "data_folder=\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26/\"\n",
    "dte = 140\n",
    "paths_gt, t0 = read_paths(data_folder, nodes, prefix=\"manual_small_\", suffix=\"-groundtruth.txt\")\n",
    "paths, _ = read_paths(output_folder, nodes, t0=t0)\n",
    "# print(f\"Avg distance {avg_distance(paths_gt, nodes):.2f}m\")\n",
    "align_paths(paths, paths_gt, align_by_first=True, align_with_minize=False)\n",
    "display(plot_fused_err(nodes, paths, paths_gt, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths, paths_gt, dte=dte, outlier_thres=0.2, outlier_yaw_thres=0.05))\n",
    "plot_fused(nodes, paths, poses_gt=None, figsize=(10,10))\n",
    "\n",
    "paths_vo, t0 = read_paths(data_folder, nodes, prefix=\"vins-mono_\", t0=t0)\n",
    "print(\"VINS-Mono\")\n",
    "align_paths(paths_vo, paths_gt)\n",
    "display(plot_fused_err(nodes, paths_vo, paths_gt, dte=dte, show=False))\n",
    "display(relative_pose_err(nodes, paths_vo, paths_gt, dte=dte))\n",
    "# plot_fused(nodes, paths_vo, poses_gt=paths_gt, figsize=(10,10))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2, 3, 4, 5]\n",
    "output_folder = f\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26//outputs/d2vins-5-async/swarm{nodes[0]}\"\n",
    "data_folder=\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26/\"\n",
    "dte = 140\n",
    "paths_gt, t0 = read_paths(data_folder, nodes, prefix=\"manual_small_\", suffix=\"-groundtruth.txt\")\n",
    "paths, _ = read_paths(output_folder, nodes, t0=t0)\n",
    "align_paths(paths, paths_gt, align_by_first=True, align_with_minize=False)\n",
    "display(plot_fused_err(nodes, paths, paths_gt, dte=dte, show=False))\n",
    "display(plot_relative_pose_err(nodes[0], nodes[1:], paths, paths_gt, dte=dte, show=False, outlier_thres=0.2, outlier_yaw_thres=0.05))\n",
    "plot_fused(nodes, paths, poses_gt=None, figsize=(10,10))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1]\n",
    "output_folder = f\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26/outputs/d2vins-calib/swarm{nodes[0]}\"\n",
    "dte = 130\n",
    "paths_gt = {}\n",
    "paths, t0 = read_paths(output_folder, nodes)\n",
    "paths_gt[1], t0 = read_path_from_csv(f\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26/manual_vo_test-groundtruth.txt\", t0)\n",
    "align_path_by_minimize(paths[1], paths_gt[1])\n",
    "display(plot_fused_err(nodes, paths, paths_gt, dte=dte, show=True, outlier_thres=0.25))\n",
    "\n",
    "# print(\"VINS-Mono\")\n",
    "# paths_vo, t0 = read_paths(data_folder, nodes, prefix=\"vins-mono\", t0=t0)\n",
    "# align_paths(paths_vo, paths_gt)\n",
    "# display(plot_fused_err(nodes, paths_vo, paths_gt, dte=dte, show=False))\n",
    "# plot_fused(nodes, paths_vo, poses_gt=paths_gt, figsize=(10,10))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Find the nearest pair of datasets\n",
    "paths_gt = {}\n",
    "t0 = None\n",
    "for i in range(1, 7):\n",
    "    paths_gt[i], t0 = read_path_from_csv(f\"/home/xuhao/data/d2slam/manual_quadcam_7inch_n3_2022_10_26/manual_small_{i}-groundtruth.txt\", t0)\n",
    "output_table = [i for i in range(0, 7)]\n",
    "output_table[0] = \"Drone\"\n",
    "\n",
    "for i in range(1, 7):\n",
    "    for j in range(i+1, 7):\n",
    "        t = find_common_times(paths_gt[i].t, paths_gt[j].t)\n",
    "        ate_pos = ATE_POS(paths_gt[i].pos_func(t), paths_gt[j].pos_func(t))\n",
    "        print(f\"i={i}, j={j} ate_pos={ate_pos}\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  },
  "vscode": {
   "interpreter": {
    "hash": "08ce52785f0fedc81003ce387e097a83d6cc9494681cd746006386992005bb71"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
